/******************** (C) COPYRIGHT 2013 STMicroelectronics ********************
* File Name          : LSM9DS1.c
* Author             : MSH Application Team
* Author             : Armando Visconti
* Version            : $Revision:$
* Date               : $Date:$
* Description        : This file provides a set of functions needed to manage the
*                      the LSM9DS1 adapter board.
* History
* Date			| Modification			| Author
* 22/10/2015            | Abstract I/O Mode, clean      | Federico Rizzardini
********************************************************************************
* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
*
* THIS SOFTWARE IS SPECIFICALLY DESIGNED FOR EXCLUSIVE USE WITH ST PARTS.
*
*******************************************************************************/


/* Includes ------------------------------------------------------------------*/
#include "lsm9ds1.h"
#include "led.h"
#include "command_interpreter.h"
#include "string.h"
#include "usb_lib.h"
#include "usb_desc.h"
#include "usb_pwr.h"
#include "ctype.h"
#include "utility.h"
#include "interruptHandler.h"
#include "i2c_mems.h"
#include "hw_config.h"
#include "stdio.h"
#include "fw_selector.h"
#include "spi_mems.h"

/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
#define SENSORS_NUMBER 3

#define ValBit(VAR,Place)         (VAR & (1<<Place))
/* Private variables ---------------------------------------------------------*/
static uint8_t usbBuff[MAX_COMMAND_LENGTH];
static uint8_t state = STOP;
static uint8_t stateOld = STOP;

static uint8_t accDataBuffer[6];
static uint8_t gyrDataBuffer[6];

static uint8_t interruptOne = 0;
static uint8_t interruptTwo = 0;
static uint8_t interruptThree = 0;


static uint8_t zoffFlag = 0;
static uint8_t echoOnFlag = 0;
static uint8_t fastest = 10; //no sensor is the fastest
static uint8_t sendFlag = 0;
static const char *acc_dbg_string[3] = {"aX", "aY", "aZ"};
static const char *gyro_dbg_string[3] = {"gX", "gY", "gZ"};
static const char *mag_dbg_string[3] = {"mX", "mY", "mZ"};

static uint16_t accODR[] = {0,10,50,119,238,476,952};
static uint16_t gyrODR[] = {0,15,60,119,238,476,952};
static uint16_t magODR[] = {0,1,2,5,10,20,40,80};

static uint16_t actualSpeed[SENSORS_NUMBER] = {0};

extern uint8_t dbSelected;

static enum io_device io_mode = USE_SPI;

#define FIFO_DEPTH 32
static char FIFO_Src = 0, Old_FIFO_Src = 0;
static char FIFO_Ovr = 0;
static char FIFO_count = 0;
static char FIFO_case = STOP;
static unsigned char state_machine = 0;
static char Fifo_depth = 32;
static char flagDeviceSelected = 0;

/* Private function prototypes -----------------------------------------------*/
void lsm9ds1_AppTick(void);
static void lsm9ds1_GPIOsInit(void);
static uint8_t Decod();
static void StateMachine(void);
static void Version(void);
static void Device(void);
static void Zoff(void);
static void Zon(void);
static void Start(void);
static void Stop(void);
static void ReadReg(void);
static void WriteReg(void);
static void GyroReadReg(void);
static void GyroWriteReg(void);
static void MagReadReg(void);
static void MagWriteReg(void);

static void Debug(void);
static void DebugA(void);
static void DebugG(void);
static void DebugM(void);
static void InterruptReceived(uint8_t interrupt);
static void DbReset(void);
static uint8_t GetFastest(uint8_t elements);

/* Read/Write Multiple regs */
static void ReadMultiReg(const char *cmd, uint8_t sensor_addr, uint8_t cs_num);
static void WriteMultiReg(const char *cmd, uint8_t sensor_addr, uint8_t cs_num);

/* Fifo functions forward declaration */
static void FifoBypass(void);
static void FifoStream(void);
static void FifoMode(void);
static void FifoRun(void);
static void FifoRead(void);
static void SendFiFoSrc(void); 
static void SendFiFoDataVector(void);
static void FifoStreamToFifo(void);
static void FifoBypassToStream(void);
static void read_acc_sensor_data(unsigned char *acc_data);
static void read_gyro_sensor_data(unsigned char *gyro_data);
static void read_mag_sensor_data(unsigned char *mag_data);
static uint8_t check_fastest_dataready(void);
static inline void Force_XYZ_axis_order(void);
static void Interrupt_Init(void);

/* Private functions ---------------------------------------------------------*/
/*******************************************************************************
* Function Name  : lsm9ds1_AppTick
* Description    : lsm9ds1 3D Application Tick Function
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
void lsm9ds1_AppTick(void)
{
  EKSTM32_LEDOn(LED3);
  if(newCommandAvailable(usbBuff)) {   
    
    Decod();
  }  
  
  StateMachine();  
}

/*******************************************************************************
* Function Name	: Decod
* Description	: decode the incoming USB data
**				:  decodification of RTX vector
**				:  *start/r:		:starts sending data
**				:  *stop/r:			:stops sending data
**				:  *ver/r:			:sends firmware version number
**				:  *dev/r:			:sends device name
**				:  *wxxdd/r:		:writes in register at address xx:(0xadr) --> the value data dd:(0xdata) 
**				:  *rxx/r:			:reads register at address xx:(0xadr)
**				:  *zon/r:			:activates tristate mode
**				:  *zoff/r:			:activates connections with devices    
**
**   Realizes the command interpreter (for general purpose commands).
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
static uint8_t Decod()
{
  if(strcmp((char*)&usbBuff[1],"VER")==0) {
    stateOld = state;
    state = VER;
  }
  else if(strcmp((char*)&usbBuff[1],"DEV")==0) {
    stateOld = state;
    state = DEV;
  }
  else if(strcmp((char*)&usbBuff[1],"ZOFF")==0) {
    stateOld = state;
    state = ZOFF;
  }
  else if(strcmp((char*)&usbBuff[1],"ZON")==0) {
    stateOld = state;
    state = ZON;
  }
  else if(strcmp((char*)&usbBuff[1],"START")==0) {
    stateOld = state;
    state = START;
  }
  else if(strcmp((char*)&usbBuff[1],"STOP")==0) {
    stateOld = state;
    state = STOP;
  } 
  else if( usbBuff[1] == 'R' ) {
    stateOld = state;
    state = R;
  }
  else if( usbBuff[1] == 'W' ) {
    stateOld = state;
    state = W;
  } 
  else if(strncmp((char*)&usbBuff[1],"GR", 2)==0) {
    stateOld = state;
    state = GR;
  }
  else if(strncmp((char*)&usbBuff[1],"GW", 2)==0) {
    stateOld = state;
    state = GW;
  } 
  else if(strncmp((char*)&usbBuff[1],"MR", 2)==0) {
    stateOld = state;
    state = MR;     
  }
  else if(strncmp((char*)&usbBuff[1],"MW", 2)==0) {
    stateOld = state;
    state = MW;        
  } 
  else if(strncmp((char*)&usbBuff[1],"MULTI-R", strlen("MULTI-R"))==0) {
    stateOld = state;
    state = RM;
  }
  else if(strncmp((char*)&usbBuff[1],"MULTI-W", strlen("MULTI-W"))==0) {
    stateOld = state;
    state = WM;
  }
  else if(strncmp((char*)&usbBuff[1],"MULTI-GR", strlen("MULTI-GR"))==0) {
    stateOld = state;
    state = GRM;
  }
  else if(strncmp((char*)&usbBuff[1],"MULTI-GW", strlen("MULTI-GW"))==0) {
    stateOld = state;
    state = GWM;
  }
  else if(strncmp((char*)&usbBuff[1],"MULTI-MR", strlen("MULTI-MR"))==0) {
    stateOld = state;
    state = MRM;
  }
  else if(strncmp((char*)&usbBuff[1],"MULTI-MW", strlen("MULTI-MW"))==0) {
    stateOld = state;
    state = MWM;
  }
  else if(strcmp((char*)&usbBuff[1],"DEBUG")==0) {
    state = DEBUG;    
  }
  else if(strcmp((char*)&usbBuff[1],"DEBUGA")==0) {
    state = DEBUGA;    
  } 
  else if(strcmp((char*)&usbBuff[1],"DEBUGG")==0) {
    state = DEBUGG;    
  }  
  else if(strcmp((char*)&usbBuff[1],"DEBUGM")==0) {
    state = DEBUGM;    
  }  
  
  //Acc FIFO Commands
  else if(strcmp((char*)&usbBuff[1],"FIFORST")==0) {
    flagDeviceSelected=0;
    state = FIFO_BYPASS;
    FIFO_case = STOP;
  }
  else if(strcmp((char*)&usbBuff[1],"FIFOMDE")==0) {
    flagDeviceSelected=0;
    state = FIFO_BYPASS;
    FIFO_case = FIFO_MODE;
  }
  else if(strcmp((char*)&usbBuff[1],"FIFOSTR")==0) {
    flagDeviceSelected=0;
    state = FIFO_BYPASS;
    FIFO_case = FIFO_STREAM;
  }
  else if(strcmp((char*)&usbBuff[1],"FIFODSTR")==0) {
    flagDeviceSelected=0;
    state = FIFO_BYPASS;
    FIFO_case = FIFO_D_STREAM;
  }   
  else if(strcmp((char*)&usbBuff[1],"FIFOBTS")==0) {
    flagDeviceSelected=0;
    state = FIFO_BYPASS;
    FIFO_case = FIFO_BYPASS_TO_STREAM;
  }
  else if(strcmp((char*)&usbBuff[1],"FIFOSTF")==0) {
    flagDeviceSelected=0;
    state = FIFO_BYPASS;
    FIFO_case = FIFO_STOP_TRIG;
  }
  
  
  // GYRO FIFO Commands
  else if(strcmp((char*)&usbBuff[1],"GFIFORST")==0) {
    flagDeviceSelected=1;
    state = FIFO_BYPASS;
    FIFO_case = STOP;    
  }
  else if(strcmp((char*)&usbBuff[1],"GFIFOMDE")==0) {
    flagDeviceSelected=1;
    state = FIFO_BYPASS;
    FIFO_case = FIFO_MODE;    
  }
  else if(strcmp((char*)&usbBuff[1],"GFIFOSTR")==0) {
    flagDeviceSelected=1;
    state = FIFO_BYPASS;
    FIFO_case = FIFO_STREAM;
  }
  else if(strcmp((char*)&usbBuff[1],"GFIFODSTR")==0) {
    flagDeviceSelected=1;
    state = FIFO_BYPASS;
    FIFO_case = FIFO_D_STREAM;
  }  
  else if(strcmp((char*)&usbBuff[1],"GFIFOBTS")==0) {
    flagDeviceSelected=1;
    state = FIFO_BYPASS;
    FIFO_case = FIFO_BYPASS_TO_STREAM;
  }  
  else if(strcmp((char*)&usbBuff[1],"GFIFOSTF")==0) {
    flagDeviceSelected=1;
    state = FIFO_BYPASS;
    FIFO_case = FIFO_STOP_TRIG;
  }
  else if(strcmp((char*)&usbBuff[1],"LIST")==0) {
    stateOld = state;    
    state = LIST;
  }  
  else if(strcmp((char*)&usbBuff[1],"SINGLE")==0) {
    stateOld = state;    
    state = SINGLE;
  }
  else if(strcmp((char*)&usbBuff[1],"ECHOON")==0) {
    stateOld = state;    
    state = ECHOON;
  }
  else if(strcmp((char*)&usbBuff[1],"ECHOOFF")==0) {
    stateOld = state;
    state = ECHOOFF;
  }  
  else if(strcmp((char*)&usbBuff[1],"LISTDEV")==0) {
    stateOld = state;
    state = LISTDEV;
  } 
  else if(strcmp((char*)&usbBuff[1],"DBRESET")==0) {
    stateOld = state;
    state = DBRESET;
  }
  else if(strncmp((char*)&usbBuff[1],"SETCOMM", 7)==0) {
    if (strcmp((char*)&usbBuff[8],"I2C")==0) {
      /* Use I2C as communication protocol */
      io_mode = USE_I2C;
      MEMS_Print("I2C\r\n", strlen("I2C\r\n"));
    } else if (strcmp((char*)&usbBuff[8],"SPI")==0) {
      /* Use SPI as communication protocol */
      io_mode = USE_SPI;
      MEMS_Print("SPI\r\n", strlen("SPI\r\n"));
    }
  }
  else {
    MEMS_Print("Unknown command\r\n", strlen("Unknown command\r\n"));
  }
  return state; 
}

/*******************************************************************************
* Function Name  : StateMachine
* Description    : StateMachiine is called from the aAppTick main loop
*                  It calls functions that implement commands selected
*                  It implements state machine for commands interpreter.
*                  If Set of available commands is extended with device specific commands
* Input          : None 
* Output         : None
* Return         : None
*******************************************************************************/
static void StateMachine(void)
{
  switch(state) {
  case VER:
    Version();
    state = stateOld;
    break;
  case DEV:
    Device();
    state = stateOld;
    break;
  case ZOFF:
    Zoff();
    state = STOP;
    stateOld = STOP;
    break;
  case ZON:
    Zon();
    state = STOP;
    stateOld = STOP;
    break;
  case START:
    if(zoffFlag) {
      Start();
    }
    break;
  case STOP:
    if(zoffFlag) {
      Stop();
    }
    break;
  case R:
    if(zoffFlag) {
      ReadReg();
      state = stateOld;
    }
    break;
  case W:
    if(zoffFlag) {
      WriteReg();
      state = stateOld;
    }
    break;
  case GR:
    if(zoffFlag) {
      GyroReadReg();
      state = stateOld;
    }
    break;
  case GW:
    if(zoffFlag) {
      GyroWriteReg();
      state = stateOld;
    }
    break;
  case MR:
    if(zoffFlag) {
      MagReadReg();
      state = stateOld;
    }
    break;
  case MW:
    if(zoffFlag) {
      MagWriteReg();
      state = stateOld;
    }
    break;
  case RM:
    if(zoffFlag) {
      ReadMultiReg("MULTI-R", ACC_GYRO_ADDRESS, CS_ACC_GYR);
      state = stateOld;
    }
    break;
  case WM:
    if(zoffFlag) {
      WriteMultiReg("MULTI-W", ACC_GYRO_ADDRESS, CS_ACC_GYR);
      state = stateOld;
    }
    break;
  case GRM:
    if(zoffFlag) {
      ReadMultiReg("MULTI-GR", ACC_GYRO_ADDRESS, CS_ACC_GYR);
      state = stateOld;
    }
    break;
  case GWM:
    if(zoffFlag) {
      WriteMultiReg("MULTI-GW", ACC_GYRO_ADDRESS, CS_ACC_GYR);
      state = stateOld;
    }
    break;
  case MRM:
    if(zoffFlag) {
      ReadMultiReg("MULTI-MR", MAG_ADDRESS, CS_MAG);
      state = stateOld;
    }
    break;
  case MWM:
    if(zoffFlag) {
      WriteMultiReg("MULTI-MW", MAG_ADDRESS, CS_MAG);
      state = stateOld;
    }
    break;
  case DEBUG:
    if(zoffFlag) {
      Debug();
    }
    break;
  case DEBUGA:
    if(zoffFlag) {
      DebugA();
    }
    break;
  case DEBUGG:
    if(zoffFlag) {
      DebugG();
    }
    break;
  case DEBUGM:
    if(zoffFlag) {
      DebugM();
    }
    break;
  case FIFO_BYPASS:
    if(zoffFlag) {
      FifoBypass();
    }
    break;
  case FIFO_STREAM:
    if(zoffFlag) {
      FifoStream();
    }
    break;
  case FIFO_MODE:
    if(zoffFlag) {
      FifoMode();
    }
    break;
  case FIFO_RUN:
    if(zoffFlag) {
      FifoRun();
    }
    break;
  case FIFO_STOP_TRIG:
    if(zoffFlag) {
      FifoStreamToFifo();
    }
    break;
  case FIFO_READ:
    if(zoffFlag) {
      FifoRead();
    }
    break;
  case FIFO_BYPASS_TO_STREAM:
    if(zoffFlag) {
      FifoBypassToStream();
    }
    break;
  case LIST:
    //It Prints the list of firmwares implemented
    PrintListOfFirmware();
    state = stateOld;
    break;
  case SINGLE:
    if(zoffFlag) {
      Debug();
      state = stateOld;
    }
    break;
  case ECHOON:
    echoOnFlag = 1;
    state = stateOld;
    break;
  case ECHOOFF:
    echoOnFlag = 0;
    state = stateOld;
    break;
  case LISTDEV:
    //It Prints the list of device supported
    PrintListOfDevice();
    state = stateOld;
    break;
  case DBRESET:
    //unselect the db   
    DbReset();
    state = STOP;
    break;
  default:     
    EKSTM32_LEDToggle(LED1);
  }
}


/*******************************************************************************
* Function Name  : Version
* Description    : Function to send Firmware version to host by USB    
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
static void Version(void)
{ 
  EmotionVer();
}

/*******************************************************************************
* Function Name  : Device
* Description    : Function to send the Device Product Code
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
static void Device(void)
{  
  char buffer[14];
  
  if (io_mode == USE_I2C)
    strcpy(buffer, "LSM9DS1\r\n");
  else
    strcpy(buffer, "LSM9DS1_SPI\r\n");
  
  MEMS_Print((uint8_t *)buffer, strlen(buffer));
}

/*******************************************************************************
* Function Name  : Zoff
* Description    : It initializes the peripherals according to the device
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
static void Zoff(void)
{
  RegisterInterrupt(InterruptReceived);
  
  if (io_mode == USE_I2C) {
    I2C_MEMS_Init();
    lsm9ds1_GPIOsInit();
    DEN_GOFF;
    CS_AON;  
  } else {
    SPI_Mems_Init();
    
    /* Configure the MEMS Chip select #2 pin */
    RCC_APB2PeriphClockCmd(CS_2_GPIO_CLK, ENABLE);
    GPIO_InitTypeDef GPIO_InitStructure;
    GPIO_InitStructure.GPIO_Pin = CS_2_GPIO_PIN;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(CS_2_GPIO_PORT, &GPIO_InitStructure);
    
    /* Deselect Mems Sensor: Chip Select high */
    SPI_MEMS_CS_2_HIGH();
  }
  
  /* Initialize LSM9DS1 adapter Interrupts */
  Interrupt_Init();

  memset(accDataBuffer, 0, 6);
  memset(gyrDataBuffer, 0, 6);  
  
  /* Force correct X/Y/Z axis order for GYRO/ACC in case device is not properly trimmed */
  Force_XYZ_axis_order();
  
  fastest = GetFastest(SENSORS_NUMBER);

  zoffFlag = 1;

  EKSTM32_LEDOn(LED2);  
}

/*******************************************************************************
* Function Name  : Zon
* Description    : It DeInitializes the peripherals according to the device
*                  Now the device is disconnected from the board
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
static void Zon(void)
{
  if (io_mode == USE_I2C) {
    I2C_MEMS_DeInit();
  } else {
    SPI_Mems_DeInit();
    GPIO_InitTypeDef GPIO_InitStructure;
    GPIO_InitStructure.GPIO_Pin = CS_2_GPIO_PIN;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
    GPIO_Init(CS_2_GPIO_PORT, &GPIO_InitStructure);
  }
  
  GPIOsDeInit();
  zoffFlag = 0;
  EKSTM32_LEDOff(LED2);
}

/*******************************************************************************
* Function Name  : Start
* Description    : Function to Send the standard output data to USB (associated to '*start' command).
*                  The system send continuosly through the USB interface
*                  a char vector composed by acceleration, interrupt and
*                  switch status.
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
static void Start(void)
{  
  static uint8_t bufferD[26];
  uint8_t sensBuf[6];
  
  sendFlag = check_fastest_dataready(); 
  
  if(sendFlag) {
    sendFlag = 0;
    
    EKSTM32_LEDToggle(LED1);    
    
    /* Preapare buffer to be sent */    
    bufferD[0]   = 's';
    bufferD[1]   = 't';
    
    /* READ ACC DATA */
    read_acc_sensor_data(sensBuf);
    bufferD[2]   = sensBuf[1]; //xh
    bufferD[3]   = sensBuf[0]; //xl
    bufferD[4]   = sensBuf[3]; //yh
    bufferD[5]   = sensBuf[2]; //yl
    bufferD[6]   = sensBuf[5]; //zh
    bufferD[7]   = sensBuf[4]; //zl
    
    /* READ GYRO DATA */
    read_gyro_sensor_data(sensBuf);
    bufferD[8]   = sensBuf[1]; //gxh
    bufferD[9]   = sensBuf[0]; //gxl
    bufferD[10]  = sensBuf[3]; //gyh
    bufferD[11]  = sensBuf[2]; //gyl
    bufferD[12]  = sensBuf[5]; //gzh
    bufferD[13]  = sensBuf[4]; //gzl   
    
    /* READ MAG DATA */
    read_mag_sensor_data(sensBuf);
    bufferD[14]   = sensBuf[1]; //gxh
    bufferD[15]   = sensBuf[0]; //gxl
    bufferD[16]  = sensBuf[3]; //gyh
    bufferD[17]  = sensBuf[2]; //gyl
    bufferD[18]  = sensBuf[5]; //gzh
    bufferD[19]  = sensBuf[4]; //gzl   
    
    
    //interrupt 1
    if(interruptOne == 1) {      
      bufferD[20]   = 0x40;
    }
    else {
      bufferD[20]   = 0x00;
    }
    
    //interrupt 2
    if(interruptTwo == 1) {
      bufferD[21]   = 0x40;
    }
    else {
      bufferD[21]   = 0x00;
    }
    
    //interrupt 3
    if(interruptThree == 1) {
      bufferD[22]   = 0x40;
    }
    else {
      bufferD[22]   = 0x00;
    }
    
    // service byte
    bufferD[23] = 0;
    
    if(SW1_BUTTON_PIN)
      bufferD[23] |= 0x01;
    
    if(SW2_BUTTON_PIN)
      bufferD[23] |= 0x02;
    
    bufferD[24]  = '\r';    
    bufferD[25]  = '\n'; 
    
    MEMS_Print(bufferD, sizeof(bufferD)); 
  }
}

/*******************************************************************************
* Function Name  : Stop
* Description    : It stops the state machine
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
static void Stop(void)
{
  state = STOP;
}

/*******************************************************************************
* Function Name  : WriteReg
* Description    : write a value in a specific sensor register
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
static void WriteReg(void)
{
  uint8_t data = 0;
  uint8_t address = 0;
  
  if(isxdigit(usbBuff[2]) && isxdigit(usbBuff[3]) && isxdigit(usbBuff[4]) && isxdigit(usbBuff[5])) {
    address = StringToChar(&usbBuff[2]);
    data = StringToChar(&usbBuff[4]);
    write_dev_reg(data, ACC_GYRO_ADDRESS, address, io_mode, CS_ACC_GYR); 
    
    if(echoOnFlag) {    
      uint8_t buffer[9];
      uint8_t response[2];
      
      address = StringToChar(&usbBuff[2]);
      
      read_dev_reg(&data, ACC_GYRO_ADDRESS, address, io_mode, CS_ACC_GYR);
      CharToString(data, response);
      
      buffer[0]  = 'R';
      buffer[1]  = usbBuff[2];
      buffer[2]  = usbBuff[3];
      buffer[3]  = 'h';
      buffer[4]  = response[0];
      buffer[5]  = response[1];
      buffer[6]  = 'h';
      buffer[7]  = '\r';
      buffer[8]  = '\n';
      
      MEMS_Print(buffer, sizeof(buffer));
    }
    
    /* If the ODR is changed, recalculate the fastest */
    if((address == CTRL_REG6_XL)||(address == CTRL_REG1_G)) {
      fastest = GetFastest(SENSORS_NUMBER);	  
    }    
  }
}

/*******************************************************************************
* Function Name  : ReadReg
* Description    : read a value in a specific sensor register
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
static void ReadReg(void)
{
  uint8_t data = 0;
  uint8_t address = 0;
  uint8_t response[2];
  uint8_t buffer[9];
  
  address = StringToChar(&usbBuff[2]); 

  read_dev_reg(&data, ACC_GYRO_ADDRESS, address, io_mode, CS_ACC_GYR);
  CharToString(data, response);
    
  buffer[0]  = 'R';
  buffer[1]  = usbBuff[2];
  buffer[2]  = usbBuff[3];
  buffer[3]  = 'h';
  buffer[4]  = response[0];
  buffer[5]  = response[1];
  buffer[6]  = 'h';
  buffer[7]  = '\r';
  buffer[8]  = '\n';
  
  MEMS_Print(buffer, sizeof(buffer));  
}

/*******************************************************************************
* Function Name  : GyroReadReg
* Description    : read a value in a specific sensor register
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
static void GyroReadReg(void)
{
  uint8_t data = 0;
  uint8_t address = 0;
  uint8_t response[2];
  uint8_t buffer[10];
  
  address = StringToChar(&usbBuff[3]);  
  read_dev_reg(&data, ACC_GYRO_ADDRESS, address, io_mode, CS_ACC_GYR);
  CharToString(data, response);
  
  buffer[0]  = 'G';
  buffer[1]  = 'R';
  buffer[2]  = usbBuff[3];
  buffer[3]  = usbBuff[4];
  buffer[4]  = 'h';
  buffer[5]  = response[0];
  buffer[6]  = response[1];
  buffer[7]  = 'h';
  buffer[8]  = '\r';
  buffer[9]  = '\n';
  
  MEMS_Print(buffer, sizeof(buffer));
}

/*******************************************************************************
* Function Name  : GyroWriteReg
* Description    : write a value in a specific sensor register
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
static void GyroWriteReg(void)
{
  uint8_t data = 0;
  uint8_t address = 0;
  
  if( isxdigit(usbBuff[3]) && isxdigit(usbBuff[4]) && isxdigit(usbBuff[5]) && isxdigit(usbBuff[6]) ) {
    address = StringToChar(&usbBuff[3]);
    data = StringToChar(&usbBuff[5]);
    write_dev_reg(data, ACC_GYRO_ADDRESS, address, io_mode, CS_ACC_GYR);
    
    if(echoOnFlag) {    
      uint8_t buffer[10];
      uint8_t response[2];
      
      address = StringToChar(&usbBuff[3]);
      read_dev_reg(&data, ACC_GYRO_ADDRESS, address, io_mode, CS_ACC_GYR);
      CharToString(data, response);
      
      buffer[0]  = 'G';
      buffer[1]  = 'R';
      buffer[2]  = usbBuff[3];
      buffer[3]  = usbBuff[4];
      buffer[4]  = 'h';
      buffer[5]  = response[0];
      buffer[6]  = response[1];
      buffer[7]  = 'h';
      buffer[8]  = '\r';
      buffer[9]  = '\n';
      
      MEMS_Print(buffer, sizeof(buffer));
    }
    
    /* If the ODR is changed, recalculate the fastest */
    if((address == CTRL_REG6_XL)||(address == CTRL_REG1_G)) {
      fastest = GetFastest(SENSORS_NUMBER);	  
    } 
  }
}

/*******************************************************************************
* Function Name  : MagReadReg
* Description    : read a value in a specific magnetometer register
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
static void MagReadReg(void)
{
  uint8_t data = 0;
  uint8_t address = 0;
  uint8_t response[2];
  uint8_t buffer[10];
  
  address = StringToChar(&usbBuff[3]);   
  read_dev_reg(&data, MAG_ADDRESS, address, io_mode, CS_MAG);
  CharToString(data, response);
  
  buffer[0]  = 'M';
  buffer[1]  = 'R';
  buffer[2]  = usbBuff[3];
  buffer[3]  = usbBuff[4];
  buffer[4]  = 'h';
  buffer[5]  = response[0];
  buffer[6]  = response[1];
  buffer[7]  = 'h';
  buffer[8]  = '\r';
  buffer[9]  = '\n';
  
  MEMS_Print(buffer, sizeof(buffer));
}

/*******************************************************************************
* Function Name  : MagWriteReg
* Description    : write a value in a specific magnetometer register
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
static void MagWriteReg(void)
{
  uint8_t data = 0;
  uint8_t address = 0;
  
  if( isxdigit(usbBuff[3]) && isxdigit(usbBuff[4]) && isxdigit(usbBuff[5]) && isxdigit(usbBuff[6]) ) {
    address = StringToChar(&usbBuff[3]);
    data = StringToChar(&usbBuff[5]);    
    write_dev_reg(data, MAG_ADDRESS, address, io_mode, CS_MAG);
    
    if(echoOnFlag) {    
      uint8_t buffer[10];
      uint8_t response[2];
      
      address = StringToChar(&usbBuff[3]);        
      
      read_dev_reg(&data, MAG_ADDRESS, address, io_mode, CS_MAG);
      CharToString(data, response);
      
      buffer[0]  = 'M';
      buffer[1]  = 'R';
      buffer[2]  = usbBuff[3];
      buffer[3]  = usbBuff[4];
      buffer[4]  = 'h';
      buffer[5]  = response[0];
      buffer[6]  = response[1];
      buffer[7]  = 'h';
      buffer[8]  = '\r';
      buffer[9]  = '\n';
      
      MEMS_Print(buffer, sizeof(buffer));
    }
    
    /* If the ODR is changed, recalculate the fastest */
    if(address == CTRL_REG1_M || address == CTRL_REG3_M) {
      fastest = GetFastest(SENSORS_NUMBER);
    }
  }
}

#define MAX_MULTI_REGS_SIZE 256

/* Read Multiple regs */
static void ReadMultiReg(const char *cmd, uint8_t sensor_addr, uint8_t cs_num)
{
  uint8_t data = 0;
  uint8_t address = 0;
  uint8_t response[2];
  uint8_t buffer[MAX_MULTI_REGS_SIZE];
  uint8_t cmd_len = strlen(cmd);
  uint8_t *bufp = &buffer[0];
  uint8_t *addrp = &usbBuff[cmd_len + 1];
  uint16_t cnt = 0;
  
  /* copy command to the answer buffer */
  strncpy((char *)bufp, cmd, cmd_len);
  bufp += cmd_len;
  cnt = cmd_len;
  
  /* parse all regs address and read them */
  while (*addrp != '\0') {
    address = StringToChar(addrp);
    read_dev_reg(&data, sensor_addr, address, io_mode, cs_num);
    CharToString(data, response);
    
    if (cnt + 6 > MAX_MULTI_REGS_SIZE)
      break;
    
    bufp[0] = addrp[0];
    bufp[1] = addrp[1];
    bufp[2] = 'h';
    bufp[3] = response[0];
    bufp[4] = response[1];
    bufp[5] = 'h';
    
    cnt += 6;
    bufp += 6;
    addrp += 2;
  }
  
  /* terminate the buffer */
  bufp[0] = '\r';
  bufp[1] = '\n';
  
  MEMS_Print(buffer, cnt + 2);
}

/* Write Multiple regs */
static void WriteMultiReg(const char *cmd, uint8_t sensor_addr, uint8_t cs_num)
{
  uint8_t data = 0;
  uint8_t address = 0;
  uint8_t buffer[MAX_MULTI_REGS_SIZE];
  uint8_t cmd_len = strlen(cmd);
  uint8_t *bufp = &buffer[0];
  uint8_t *addrp = &usbBuff[cmd_len + 1];
  
  /* copy command to the answer buffer */
  strncpy((char *)bufp, cmd, cmd_len);
  bufp += cmd_len;
  
  /* parse all regs address and write them */
  while (*addrp != '\0') {
    if(isxdigit(addrp[0]) && isxdigit(addrp[1]) && isxdigit(addrp[2]) && isxdigit(addrp[3])) {
      
      address = StringToChar(addrp);
      data = StringToChar(addrp + 2);
      write_dev_reg(data, sensor_addr, address, io_mode, cs_num);
      
      /* display register values as just set */
      if(echoOnFlag) {
        uint8_t buffer[9];
        uint8_t response[2];
        
        address = StringToChar(addrp);
        
        read_dev_reg(&data, sensor_addr, address, io_mode, cs_num);
        CharToString(data, response);
        
        buffer[0]  = 'W';
        buffer[1]  = addrp[0];
        buffer[2]  = addrp[1];
        buffer[3]  = 'h';
        buffer[4]  = response[0];
        buffer[5]  = response[1];
        buffer[6]  = 'h';
        buffer[7]  = '\r';
        buffer[8]  = '\n';
        
        MEMS_Print(buffer, sizeof(buffer));
      }
      
      /* if the ODR is changed, recalculate the fastest */
      if(address == CTRL_REG1_M) {
        fastest = GetFastest(SENSORS_NUMBER);
      }
      addrp += 4;
    }
  }
}

/*******************************************************************************
* Function Name  : Debug
* Description    : Function to Send the debug output data to USB (associated to '*debug' command).
*                  The system send continuosly through the USB interface
*                  a readable string containg acquired data
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
void Debug(void)
{
  uint8_t buffer[10];
  int16_t sensBuf[3];
  uint8_t i;
  
  sendFlag = 0;
  sendFlag = check_fastest_dataready();
  
  if(sendFlag) {   
    sendFlag = 0;   
    
    EKSTM32_LEDToggle(LED1);
    
    sendFlag = 0;
    
    EKSTM32_LEDToggle(LED1);    
    
    /* prepare DEBUG ACC buffer data */
    read_acc_sensor_data((uint8_t *)sensBuf);
    for (i = 0; i < 3; i++) {
      sprintf((char*)buffer,"%s=%+5d\t\0", acc_dbg_string[i], sensBuf[i]);
      MEMS_Print((unsigned char*)buffer, strlen((char*)buffer));
    }
    
    /* prepare DEBUG GYRO buffer data */
    read_gyro_sensor_data((uint8_t *)sensBuf);
    for (i = 0; i < 3; i++) {
      sprintf((char*)buffer,"%s=%+5d\t\0", gyro_dbg_string[i], sensBuf[i]);
      MEMS_Print((unsigned char*)buffer, strlen((char*)buffer));
    }
    
    /* prepare DEBUG MAG buffer data */
    read_mag_sensor_data((uint8_t *)sensBuf);
    for (i = 0; i < 3; i++) {
      sprintf((char*)buffer,"%s=%+5d\t\0", mag_dbg_string[i], sensBuf[i]);
      MEMS_Print((unsigned char*)buffer, strlen((char*)buffer));
    }
    
    /* newline */
    sprintf((char*)buffer,"\r\n\0");  
    MEMS_Print((unsigned char*)buffer, strlen((char*)buffer));
  }
}

/*******************************************************************************
* Function Name  : DebugG
* Description    : Function to Send the debug output data to USB (associated to '*debug' command).
*                  The system send continuosly through the USB interface
*                  a readable string containg acquired data
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
static void DebugG(void)
{
  uint8_t buffer[10];
  int16_t gyroBuf[3];
  uint8_t i;  
  
  sendFlag = check_fastest_dataready();
  
  if(sendFlag) {   
    sendFlag = 0;   
    EKSTM32_LEDToggle(LED1);
    
    /* prepare DEBUG GYRO buffer data */
    read_gyro_sensor_data((uint8_t *)gyroBuf);
    for (i = 0; i < 3; i++) {
      sprintf((char*)buffer,"%s=%+5d\t\0", gyro_dbg_string[i], gyroBuf[i]);
      MEMS_Print((unsigned char*)buffer, strlen((char*)buffer));
    }
    
    /* newline */
    sprintf((char*)buffer,"\r\n\0");  
    MEMS_Print((unsigned char*)buffer, strlen((char*)buffer));
  }
}

/*******************************************************************************
* Function Name  : DebugM
* Description    : Function to Send the debug output data to USB (associated to '*debug' command).
*                  The system send continuosly through the USB interface
*                  a readable string containg acquired data
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
static void DebugM(void)
{
  uint8_t buffer[10];
  int16_t magBuf[3];
  uint8_t i;  
  
  sendFlag = check_fastest_dataready();
  
  if(sendFlag) {   
    sendFlag = 0;   
    EKSTM32_LEDToggle(LED1);
    
    /* prepare DEBUG GYRO buffer data */
    read_mag_sensor_data((uint8_t *)magBuf);
    for (i = 0; i < 3; i++) {
      sprintf((char*)buffer,"%s=%+5d\t\0", mag_dbg_string[i], magBuf[i]);
      MEMS_Print((unsigned char*)buffer, strlen((char*)buffer));
    }
    
    /* newline */
    sprintf((char*)buffer,"\r\n\0");  
    MEMS_Print((unsigned char*)buffer, strlen((char*)buffer));
  }
}

/*******************************************************************************
* Function Name  : DebugA
* Description    : Function to Send the debug output data to USB (associated to '*debug' command).
*                  The system send continuosly through the USB interface
*                  a readable string containg acquired data
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
static void DebugA(void)
{
  uint8_t buffer[10];
  int16_t accBuf[3];
  uint8_t i;
  
  sendFlag = 0;
  sendFlag = check_fastest_dataready();
  
  if(sendFlag) {
    sendFlag = 0;   
    EKSTM32_LEDToggle(LED1);
    
    /* prepare DEBUG ACC buffer data */
    read_acc_sensor_data((uint8_t *)accBuf);
    for (i = 0; i < 3; i++) {
      sprintf((char*)buffer,"%s=%+5d\t\0", acc_dbg_string[i], accBuf[i]);
      MEMS_Print((unsigned char*)buffer, strlen((char*)buffer));
    }
    
    /* newline */
    sprintf((char*)buffer,"\r\n\0");  
    MEMS_Print((unsigned char*)buffer, strlen((char*)buffer));
  }
}

/*******************************************************************************
* Function Name  : Interrupt_Init
* Description    : Initializes the LSM9DS1 interrupt Pin 3
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
static void Interrupt_Init(void)
{
  GPIO_InitTypeDef GPIO_InitStructure;
  EXTI_InitTypeDef EXTI_InitStructure;
  NVIC_InitTypeDef NVIC_InitStructure;
  
  /* Interrupt 3 */
  /* Enable Button GPIO clock */
  RCC_APB2PeriphClockCmd(INT3_CLK | RCC_APB2Periph_AFIO, ENABLE);
  
  /* Configure Button pin as input floating */
  GPIO_InitStructure.GPIO_Pin = INT3_PIN;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
  GPIO_Init(INT3_PORT, &GPIO_InitStructure);
  
  /* Connect Button EXTI Line to Button GPIO Pin */
  GPIO_EXTILineConfig(INT3_PORT_SOURCE, INT3_PIN_SOURCE);
  
  /* Configure Button EXTI line */
  EXTI_InitStructure.EXTI_Line = INT3_EXTI_LINE;
  EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
  EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising_Falling;
  
  EXTI_InitStructure.EXTI_LineCmd = ENABLE;
  EXTI_Init(&EXTI_InitStructure);
  
  /* Enable and set Button EXTI Interrupt to the lowest priority */
  NVIC_InitStructure.NVIC_IRQChannel = INT3_IRQn;
  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x0F;
  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x0F;
  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
  
  NVIC_Init(&NVIC_InitStructure);
}

/*******************************************************************************
* Function Name  : InterruptReceived
* Description    : Interrupt One and Two Handler
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
static void InterruptReceived(uint8_t interrupt)
{
  uint8_t data;
  
  switch (interrupt){
  case INTERRUPTONE:   
    data = GPIO_ReadInputDataBit(INT1_PORT, INT1_PIN);
    if(data == 0x01)
      interruptOne = 1;
    else
      interruptOne = 0;	  
    break;
    
  case INTERRUPTTWO:                      
    data = GPIO_ReadInputDataBit(INT2_PORT, INT2_PIN);                        
    if(data == 0x01)                        
      interruptTwo = 1;                        
    else                        
      interruptTwo = 0;                        
    break;	  
    
  case INTERRUPTTHREE:
    data = GPIO_ReadInputDataBit(INT3_PORT, INT3_PIN);
    
    if(data == 0x01)
      interruptThree = 1;
    else
      interruptThree = 0;
    break;
  }
}

/*******************************************************************************
* Function Name  : DbReset
* Description    : It reset the firmware selectet. 
*                : A new firmware can be now selected
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
static void DbReset(void)
{  
  dbSelected = 0;
  Zon();
  EKSTM32_LEDOff(LED1);
  EKSTM32_LEDOff(LED2);
  EKSTM32_LEDOff(LED3);   
}

/*******************************************************************************
* Function Name  : GetFastest(uint8_t elements)
* Description    : It gets the fastest odr
* Input          : bufferNone
* Output         : None
* Return         : None
*******************************************************************************/
uint8_t GetFastest(uint8_t elements)
{
  uint8_t ret = 0;
  uint8_t i = 0;
  uint8_t gyro_data, acc_data, mag_data;
  
  /* Read Gyro ODR */
  read_dev_reg(&gyro_data, ACC_GYRO_ADDRESS, CTRL_REG1_G, io_mode, CS_ACC_GYR);
  gyro_data = (gyro_data >> 5) & 0x7;
  actualSpeed[GYROSCOPE] = gyrODR[gyro_data];
  
  /* Read Acc ODR */
  read_dev_reg(&acc_data, ACC_GYRO_ADDRESS, CTRL_REG6_XL, io_mode, CS_ACC_GYR);
  acc_data = (acc_data >> 5) & 0x7;
  actualSpeed[ACCELEROMETER] = accODR[acc_data];
  
  /* Read Mag ODR */
  read_dev_reg(&mag_data, MAG_ADDRESS, CTRL_REG3_M, io_mode, CS_MAG);
  if ((mag_data & 0x2) == 0x0) {
    /* not in power down */
    read_dev_reg(&mag_data, MAG_ADDRESS, CTRL_REG1_M, io_mode, CS_MAG);
    mag_data = (mag_data >> 2) & 0x7;
    actualSpeed[MAGNETOMETER] = magODR[mag_data];
  } else {
    /* power down */
    actualSpeed[MAGNETOMETER] = 0;
  }
  
  /* check which sensor is faster */
  ret = 0;
  for(i = 0; i < elements; i++) {	
    if(actualSpeed[ret] < actualSpeed[i])
      ret = i;
  }
  
  return ret;
}

/*******************************************************************************
* Function Name  : lsm9ds1_GPIOsInit
* Description    : Initializes the GPIO Pins
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
static void lsm9ds1_GPIOsInit(void)
{ 
  GPIO_InitTypeDef  GPIO_InitStructure;
  
  /*SDO and CS_G are already handled in the I2C initialization function (see i2c_mems.c)*/
    
  /* Enable the DEN_G_GPIO Clock */
  RCC_APB2PeriphClockCmd(DEN_G_GPIO_CLK, ENABLE);

  /* Configure the DEN_G_GPIO pin */
  GPIO_InitStructure.GPIO_Pin = DEN_G_GPIO_PIN;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; 
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;

  GPIO_Init(DEN_G_GPIO_PORT, &GPIO_InitStructure);
  
  /* Enable the CS_A_GPIO Clock */
  RCC_APB2PeriphClockCmd(CS_A_GPIO_CLK, ENABLE);

  /* Configure the GPIO_CS pin */
  GPIO_InitStructure.GPIO_Pin = CS_A_GPIO_PIN;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; 
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;

  GPIO_Init(CS_A_GPIO_PORT, &GPIO_InitStructure);
}

///////////////////////////////// Gyro FIFO ////////////////////////////////////
/*******************************************************************************
* Function Name  : FifoStopTrig
* Description    : 
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
static void FifoStreamToFifo(void)
{
  uint8_t data = 0;
  
  switch (state_machine) {
  case 0:
    /* Clear Interrupt if LIR enabled */
    read_dev_reg(&data, ACC_GYRO_ADDRESS, G_INT1_SRC, io_mode, CS_ACC_GYR);
    read_dev_reg(&data, ACC_GYRO_ADDRESS, INT_GEN_SRC_XL, io_mode, CS_ACC_GYR);
    
    /* Fifo Enable */
    read_dev_reg(&data, ACC_GYRO_ADDRESS, CTRL_REG9, io_mode, CS_ACC_GYR);
    data |= CTRL_REG9_FIFO_EN;  	
    write_dev_reg(data, ACC_GYRO_ADDRESS, CTRL_REG9, io_mode, CS_ACC_GYR);
    
    read_dev_reg(&data, ACC_GYRO_ADDRESS, FIFO_CONTROL, io_mode, CS_ACC_GYR);
    data = (data & FIFO_CONTROL_WTMSAMP) | DEF_FIFO_CTRL_STREAM_TO_FIFO;
    write_dev_reg(data, ACC_GYRO_ADDRESS, FIFO_CONTROL, io_mode, CS_ACC_GYR);
    
    SendFiFoSrc();
    
    /* Read Trigger bit */       
    state_machine = 1;
    break;
    
    /*
    * In this state we poll fot the OVR bit. If set switch to state 2.
    */	
  case 1:  
    read_dev_reg((uint8_t *)&FIFO_Src, ACC_GYRO_ADDRESS, FIFO_SOURCE, io_mode, CS_ACC_GYR);
    FIFO_Ovr = FIFO_Src & FIFO_SOURCE_OVRN;
    
    /* If FIFO Orun and Interrupt */
    if (FIFO_Ovr)
      state_machine = 2;
    
    /* If new sample in FIFO */
    if (FIFO_Src != Old_FIFO_Src) {
      SendFiFoSrc();
      Old_FIFO_Src = FIFO_Src;
    }
    
    break;
    
    /*
    * In this state we read all entries in the FIFO. Then we go back to state 1.
    */	
  case 2:
    /* Set next FIFO case */
    FIFO_case = FIFO_STOP_TRIG;
    state = FIFO_READ;
    
    /* reset samples counter */
    FIFO_count = 0;
    state_machine = 1;
    break;
    
  case 3:
    state = STOP;
    break;
    
  default: break;
  } 
}

/*******************************************************************************
* Function Name  : FifoBypassToStream
* Description    : 
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
static void FifoBypassToStream(void)
{
  uint8_t data;
  /* Clear Interrupt if LIR enabled */
  read_dev_reg(&data, ACC_GYRO_ADDRESS, G_INT1_SRC, io_mode, CS_ACC_GYR);
  read_dev_reg(&data, ACC_GYRO_ADDRESS, INT_GEN_SRC_XL, io_mode, CS_ACC_GYR);
  
  /* Interrupt spend 1/2 samples to go low */				
  read_dev_reg(&data, ACC_GYRO_ADDRESS, CTRL_REG9, io_mode, CS_ACC_GYR);
  data |= CTRL_REG9_FIFO_EN;  	
  write_dev_reg(data, ACC_GYRO_ADDRESS, CTRL_REG9, io_mode, CS_ACC_GYR);       
  
  read_dev_reg(&data, ACC_GYRO_ADDRESS, FIFO_CONTROL, io_mode, CS_ACC_GYR);
  data = (data & FIFO_CONTROL_WTMSAMP) | DEF_FIFO_CTRL_BYPASS_TO_STREAM;
  write_dev_reg(data, ACC_GYRO_ADDRESS, FIFO_CONTROL, io_mode, CS_ACC_GYR);
  
  /* Send FIFO Source */
  SendFiFoSrc();
  /* In FIFO MODE, case FIFO STREAM is executed only one time */
  state = FIFO_RUN;  
}

/*******************************************************************************
* Function Name  : FifoBypass
* Description    : 
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
static void FifoBypass(void)
{
  uint8_t data;
  
  /* Clear Interrupt if LIR enabled */
  read_dev_reg(&data, ACC_GYRO_ADDRESS, G_INT1_SRC, io_mode, CS_ACC_GYR);
  read_dev_reg(&data, ACC_GYRO_ADDRESS, INT_GEN_SRC_XL, io_mode, CS_ACC_GYR);
  
  /* Reset FIFO_STOP_Trigger State Machine */
  state_machine = 0;
  
  /* Bypass Mode */
  read_dev_reg(&data, ACC_GYRO_ADDRESS, FIFO_CONTROL, io_mode, CS_ACC_GYR);
  data = (data & FIFO_CONTROL_WTMSAMP) | DEF_FIFO_CTRL_BYPASS;
  write_dev_reg(data, ACC_GYRO_ADDRESS, FIFO_CONTROL, io_mode, CS_ACC_GYR);       

  /* Disable FIFO */
  read_dev_reg(&data, ACC_GYRO_ADDRESS, CTRL_REG9, io_mode, CS_ACC_GYR);
  data = data & (~CTRL_REG9_FIFO_EN); 	
  write_dev_reg(data, ACC_GYRO_ADDRESS, CTRL_REG9, io_mode, CS_ACC_GYR); 
  
  /* Send FIFO Source */
  SendFiFoSrc();
  
  read_dev_reg(&data, ACC_GYRO_ADDRESS, CTRL_REG9, io_mode, CS_ACC_GYR);
  data = data & CTRL_REG9_STOP_ON_WTM;
  if(data){
    read_dev_reg(&data, ACC_GYRO_ADDRESS, FIFO_CONTROL, io_mode, CS_ACC_GYR);
    Fifo_depth = (data & FIFO_CONTROL_WTMSAMP) + 1;	
  } 
  else { 
    Fifo_depth = 32;
  }
  
  /* Return to previous FIFO CASE */
  state = FIFO_case;    
}


/*******************************************************************************
* Function Name  : FifoStream
* Description    : 
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
static void FifoStream(void)
{
  uint8_t data;
  
  /* Fifo Enable */
  read_dev_reg(&data, ACC_GYRO_ADDRESS, CTRL_REG9, io_mode, CS_ACC_GYR);
  data |= CTRL_REG9_FIFO_EN;  	
  write_dev_reg(data, ACC_GYRO_ADDRESS, CTRL_REG9, io_mode, CS_ACC_GYR);

  read_dev_reg(&data, ACC_GYRO_ADDRESS, FIFO_CONTROL, io_mode, CS_ACC_GYR);
  data = (data & FIFO_CONTROL_WTMSAMP) | DEF_FIFO_CTRL_STREAM;
  write_dev_reg(data, ACC_GYRO_ADDRESS, FIFO_CONTROL, io_mode, CS_ACC_GYR);
  
  /* Send FIFO Source */
  SendFiFoSrc();
  state = FIFO_RUN;
}

/*******************************************************************************
* Function Name  : FifoMode
* Description    : 
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
static void FifoMode(void)
{
  uint8_t data;
  
  /* Fifo Enable */
  read_dev_reg(&data, ACC_GYRO_ADDRESS, CTRL_REG9, io_mode, CS_ACC_GYR);
  data |= CTRL_REG9_FIFO_EN;  	
  write_dev_reg(data, ACC_GYRO_ADDRESS, CTRL_REG9, io_mode, CS_ACC_GYR);      

  read_dev_reg(&data, ACC_GYRO_ADDRESS, FIFO_CONTROL, io_mode, CS_ACC_GYR);
  data = (data & FIFO_CONTROL_WTMSAMP) | DEF_FIFO_CTRL_FIFO_MODE;
  write_dev_reg(data, ACC_GYRO_ADDRESS, FIFO_CONTROL, io_mode, CS_ACC_GYR);  
  
  /* Send FIFO Source */
  SendFiFoSrc(); 
  state = FIFO_RUN;    
}

/*******************************************************************************
* Function Name  : FifoRun
* Description    : 
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
static void FifoRun(void)
{
  uint8_t data;
  read_dev_reg(&data, ACC_GYRO_ADDRESS, FIFO_SOURCE, io_mode, CS_ACC_GYR);
  FIFO_Src = data;
  
  FIFO_Ovr = FIFO_Src & FIFO_SOURCE_OVRN;
  
  if(FIFO_Src != Old_FIFO_Src) {
    SendFiFoSrc();
    Old_FIFO_Src = FIFO_Src; 
  }
  
  if (FIFO_Ovr) {
    /* Set next FIFO case */
    FIFO_case = FIFO_RUN;
    state = FIFO_READ;
    
    FIFO_count = 0;	
  } 
}

/*******************************************************************************
* Function Name  : FifoRead
* Description    : 
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
static void FifoRead(void)
{
  //FIFO READ : The system send 32 accelerations samples 
  //through the USB interface
  //(FIFO content) 
  if (FIFO_count < Fifo_depth) {
    SendFiFoDataVector();
    
    FIFO_count++;      
  }
  else 
    state = FIFO_case; // Return to previous FIFO CASE  
}

/*******************************************************************************
* Function Name  : SendFiFoSrc
* Description    : Function to Send the standard output data to USB (associated to '*FIFO' command).
*                  The system send continuosly through the USB interface
*                  a char vector composed by, interrupt,
*                  FIFO_CONTROL and FIFO_SOURCE
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
static void SendFiFoSrc(void)
{
  uint8_t FIFO_Vector[14];
  uint8_t data;
  
  FIFO_Vector[0] = 's';
  FIFO_Vector[1] = 't';
  
  FIFO_Vector[2] = 0;
  FIFO_Vector[3] = 0;
  FIFO_Vector[4] = 0;  
  FIFO_Vector[5] = 0;
  FIFO_Vector[6] = 0;
  FIFO_Vector[7] = 0;
  
  //interrupt 1
  if(interruptOne == 1) {
    FIFO_Vector[8] = 0x40;
  }
  else {
    FIFO_Vector[8] = 0x00;
  }
  
  //interrupt 1
  if(interruptTwo == 1) {
    FIFO_Vector[9] = 0x40;
  }
  else {
    FIFO_Vector[9] = 0x00;
  }
  
  read_dev_reg(&data, ACC_GYRO_ADDRESS, FIFO_CONTROL, io_mode, CS_ACC_GYR);
  FIFO_Vector[10] = data;
  read_dev_reg(&data, ACC_GYRO_ADDRESS, FIFO_SOURCE, io_mode, CS_ACC_GYR);
  FIFO_Vector[11] = data;
  
  FIFO_Vector[12] = '\r';
  FIFO_Vector[13] = '\n';
  
  MEMS_Print(FIFO_Vector, sizeof(FIFO_Vector)); 
}

/*******************************************************************************
* Function Name  : SendFiFoDataVector
* Description    : Function to Send the standard output data to USB (associated to '*FIFO' command).
*                  The system send continuosly through the USB interface
*                  a char vector composed by, FIFO Data, interrupt,
*                  FIFO_CONTROL and FIFO_SOURCE
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
static void SendFiFoDataVector(void)
{
  uint8_t FIFO_Data_Vector[14];
  uint8_t data;
  uint8_t sensDataBuf[6];
  
  FIFO_Data_Vector[0] =	's';
  FIFO_Data_Vector[1] =	't';
  
  if (flagDeviceSelected == 1) {
    /*
    * Gyro only case does not exist. So, read OUT the accelerometer values
    * otherwise FIFO ptr are not incremented.
    */
    read_acc_sensor_data(sensDataBuf);
    
    /* prepare GYRO data */
    read_gyro_sensor_data(sensDataBuf);
    FIFO_Data_Vector[2] = sensDataBuf[1];
    FIFO_Data_Vector[3] = sensDataBuf[0];
    FIFO_Data_Vector[4] = sensDataBuf[3];
    FIFO_Data_Vector[5] = sensDataBuf[2];
    FIFO_Data_Vector[6] = sensDataBuf[5];
    FIFO_Data_Vector[7] = sensDataBuf[4];  
  } else {
    /* prepare ACC data */
    read_acc_sensor_data(sensDataBuf);
    FIFO_Data_Vector[2] = sensDataBuf[1];
    FIFO_Data_Vector[3] = sensDataBuf[0];
    FIFO_Data_Vector[4] = sensDataBuf[3];
    FIFO_Data_Vector[5] = sensDataBuf[2];
    FIFO_Data_Vector[6] = sensDataBuf[5];
    FIFO_Data_Vector[7] = sensDataBuf[4];
  }
  
  if(interruptOne == 1) {
    FIFO_Data_Vector[8] = 0x40;
  }
  else {
    FIFO_Data_Vector[8] = 0x00;
  }
  
  if(interruptTwo == 1) {
    FIFO_Data_Vector[9] = 0x40;
  }
  else {
    FIFO_Data_Vector[9] = 0x00;
  }
  
  read_dev_reg(&data, ACC_GYRO_ADDRESS, FIFO_CONTROL, io_mode, CS_ACC_GYR);
  FIFO_Data_Vector[10] = data;
  read_dev_reg(&data, ACC_GYRO_ADDRESS, FIFO_SOURCE, io_mode, CS_ACC_GYR);
  FIFO_Data_Vector[11] = data;
  
  FIFO_Data_Vector[12] = '\r';
  FIFO_Data_Vector[13] = '\n';
  
  MEMS_Print(FIFO_Data_Vector, sizeof(FIFO_Data_Vector)); 
}

/* Read ACC DATA (First MSB and then LSB) */
static void read_acc_sensor_data(unsigned char *acc_data)
{
  read_dev_mem(acc_data, ACC_GYRO_ADDRESS, OUT_X_L, 6, io_mode, CS_ACC_GYR);
}

/* Read GYRO DATA (First MSB and then LSB) */
static void read_gyro_sensor_data(unsigned char *gyro_data)
{
  read_dev_mem(gyro_data, ACC_GYRO_ADDRESS, OUT_GX_L, 6, io_mode, CS_ACC_GYR);
}

/* Read MAG DATA (First MSB and then LSB) */
static void read_mag_sensor_data(unsigned char *mag_data)
{
  read_dev_mem(mag_data, MAG_ADDRESS, OUT_MX_L, 6, io_mode, CS_MAG);
}

/* Return whether data is ready on fastest sensor */
static uint8_t check_fastest_dataready(void)
{
  uint8_t dataReady = 0;
  
  switch (fastest) {
  case GYRO_IS_FASTEST:
    read_dev_reg(&dataReady, ACC_GYRO_ADDRESS, G_STATUS_REG, io_mode, CS_ACC_GYR);
    if(ValBit(dataReady, GYRO_DATAREADY_BIT)) {
      return(1);
    }
    break;
    
  case ACC_IS_FASTEST:
    read_dev_reg(&dataReady, ACC_GYRO_ADDRESS, A_STATUS_REG, io_mode, CS_ACC_GYR);
    if(ValBit(dataReady, A_DATAREADY_BIT)) {
      return(1);
    }
    break;
    
  case MAG_IS_FASTEST:
    read_dev_reg(&dataReady, MAG_ADDRESS, STATUS_REG_M, io_mode, CS_MAG);
    if(ValBit(dataReady, M_DATAREADY_BIT)) {
      return(1);
    }
    break;
    
  default:
    break;
  }
  
  return(0);
}

/* Force correct X/Y/Z axis order for GYRO/ACC in case device is not properly trimmed */
static inline void Force_XYZ_axis_order(void)
{
  uint8_t data = 0;

  /* Set proper register page */
  data = 0x0;
  write_dev_reg(data, ACC_GYRO_ADDRESS, 0x0, io_mode, CS_ACC_GYR);
  write_dev_reg(data, ACC_GYRO_ADDRESS, 0x13, io_mode, CS_ACC_GYR);

  /* Change X/Y/Z axis order */
  data = 0x7A;
  write_dev_reg(data, ACC_GYRO_ADDRESS, 0x4D, io_mode, CS_ACC_GYR);

  read_dev_reg(&data, ACC_GYRO_ADDRESS, 0x70, io_mode, CS_ACC_GYR);
  data = (data & 0xFC) | 0x01;
  write_dev_reg(data, ACC_GYRO_ADDRESS, 0x70, io_mode, CS_ACC_GYR);
}

/**
  * @}
  */ 

/**
  * @}
  */ 

/******************* (C) COPYRIGHT 2012 STMicroelectronics *****END OF FILE****/
